/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef COORDINATETRAJECTORY_H
#define COORDINATETRAJECTORY_H
#include "MacroDefine/GeneralDefine.h"
#include <kdl/frames.hpp>
#include <trajectory_composite.hpp>
#include "AxisDefines.h"
#include <QQueue>
#include "motionmessage.h"
#include <QMutex>

#define D_MICRO_ANGLE_TIME 1.0
#define D_BEZIER_LENGTH_RATIO 0.38
#define D_SPLINE_ALLOW_ANGLE 120
#define D_SPLINE_ALLOW_CONFILIT_ANGLE 22
#define D_SPLINE_ALLOW_DISTANCE 0.05
#define D_SPLINE_ALLOW_ANGLE_MOVE_RATIO 2

enum E_LINE_VELOCITY_STYLE
{
    E_LINE_VELOCITY_STYLE_ZERO_ZERO,
    E_LINE_VELOCITY_STYLE_CHANGE_CONSTANT,
    E_LINE_VELOCITY_STYLE_CONSTANT_CHANGE,
    E_LINE_VELOCITY_STYLE_CHANGE_CONSTANT_ZERO
};

using namespace KDL;

struct TrajeAttribute
{
    KDL::Trajectory_Composite* trajComp;
    double timeDuration;
    int cnt;
    double endCutoffTime;
    double accumulatedTime;

};
/**
 * @brief The CoordinateTrajectory class 空间轨迹轴的控制
 */
class CoordinateTrajectory
{
public:
    CoordinateTrajectory(int robotIdIn, double deltaTimeIn, JointAttribute axisAttributeIn,
                         MotionMessage *motionMessageIn,double rotate_eqradiusIn,double allowVelFeedbackBiasRatioIn,
                         double rotate_maxVelIn=0, double rotate_maxAccIn=0);
public:
    int setRobotCoordinateConfig(struct RobotCoordinateAxisConfig coordinateConfigIn);
    bool getMixRatioOut(double & mixedRatioOut);
    int setRotateEqradius(double rotate_eqradiusIn);
    int getVehicleTrajectoryControlParameter(struct VehicleTrajectoryControlParameter &infoOut);//m/s
    int setVehicleTrajectoryControlParameter(struct VehicleTrajectoryControlParameter infoIn,int &returnFlag);//m/s
    int setVelocityStyle(E_VELOCITY_TYPE velocityStypeIn);
    /**
     * @brief getTrajectoryPoint_steeringAngle
     * @return degree 转向角度
     */
    double getTrajectoryPoint_steeringAngle();
    int setSteeringMotionParameter(double maxVelIn,double maxAccIn,bool isSteeringValidIn);
    bool isLastLineVelocityZero();
//    int revertTime(double timeDuration);//退回时间ｓ.应用于agv壁障。
    int setNoRatateMoveAngle(double angleIn);
    int setAxisFeedbackVelocity(double transVelIn,double rotateVelIn);
    int setMaxVelocity(double maxVelIn);
    int setAccelerationRatio(double ratioIn);
    int setAcceleration(double accelerationIn);
    int setAcceleration_rotate(double accelerationIn);
    int getSegmentMotionId(E_MOTION_ID &idIn);
//    double getTrajectoryRemainningTime();
   int processLineInstruction(MoveParameter moveParameter);
   int processCircleInstruction(MoveParameter moveParameter);
   int processCartesianContinousInstruction(QVector<MoveParameter> moveParameterArry, int isLastPointMove);

    int loadAxisAttribute(JointAttribute axisAttributeIn,double rotate_eqradiusIn,double rotate_maxVelIn,
                          double rotate_maxAccIn);


    int returnAxisCommandData(PointPro &returnedPosition, Twist &returnedVel, Twist &returnedAcc, Twist &returnedTorque );
    int returnAxisFeedbackData(PointPro &returnedPosition, Twist &returnedVel, Twist &returnedTorque );
    int returnAxisFollowOffsetData(PointPro &returnedPosition, Twist &returnedVel,Twist &returnedTorque );

    int setTimeAxisScale(double ratio);
    int setNewTimeAxisVelLimitRatio(double scaleIn);
    int restoreTimeAxisVelLimitRatio(double scaleIn);
    int resetTimeAxisVelLimitRatio();

    int setDeltaTime(double time);

    int moveOneStep();
    int moveOneStepByTrajIndex(int trajIndex);
    int moveOneStep_secondTraj();
    int moveOneStep_mixedPosition();
    int clearTraj();
    int clearTraj_front();


    int moveOneStepByPosition(PointPro positionIn);

    int moveOneStepByPV(PointPro positionIn, Twist velIn );

    int moveOneStepByPVAT(PointPro positionIn, Twist velIn, Twist accIn, Twist torqueIn);

    //robot used
    int lineMove(Frame positionIn, double speedPercent, double &timeDurationResult,
                 int cntIn=0, double minMixDurationIn=-1);
    /**
     * @brief lineMove_3segment 通过velProfileArry的方法实现。timeDuration逻辑有问题。not used
     * @param isPositiveDirection
     * @param positionIn
     * @param speedPercent
     * @param timeDurationResult
     * @return  //no tested
     */
//    int lineMove_3segment(bool isPositiveDirection, Frame positionIn, double speedPercent, double &timeDurationResult);
    int lineMove_3segment_composed(bool isPositiveDirection, bool isVelocitySpline, VehiclePosition positionIn,
                                   double speedPercent, double &timeDurationResult, bool isEndRotate=true);

    int lineMove_maccrum_2segment_composed(bool isVelocitySpline, VehiclePosition positionIn, double speedPercent,
                                   double &timeDurationResult);

    int bezierMove_noRotateFromCurrentPos(E_LINE_VELOCITY_STYLE velStyle, bool isPositiveDirection, VehiclePosition positionIn,
                                   double speedPercent, double &timeDurationResult);
    int bezierMove_withControlPoints(E_LINE_VELOCITY_STYLE velStyle, bool isPositiveDirection,
                                     VehiclePosition targetPositionIn,VehiclePosition pos1,VehiclePosition pos2,
                                   double speedPercent, double &timeDurationResult);


    /**
     * @brief lineMove_only_rotation //纯粹旋转不能用spline，时间计算不对。
     * @param isVelocitySpline
     * @param positionIn
     * @param speedPercent
     * @param timeDurationResult
     * @return
     */
    int lineMove_only_rotation(bool isVelocitySpline, VehiclePosition positionIn,
                               double speedPercent, double &timeDurationResult);
    /**
     * @brief lineMove_4segment_composed hualei need todo ,将直线段拆分成梯形加速、匀速、Ｓ减速，使停止更加准确。
     * @param isPositiveDirection
     * @param positionIn
     * @param speedPercent
     * @param timeDurationResult
     * @return
     */
    int lineMove_4segment_composed(bool isPositiveDirection, Frame positionIn, double speedPercent,
                                   double &timeDurationResult);
    //机械手用robot used
    int lineContinuousMove_mixedCNT(QVector<Frame> positionIn, QVector<double> speedPercent,
                                    QVector<double> &timeDurationResult, QVector<double> cntList,
                                    double minMixDurationIn, QVector<int> accListIn);
    int lineContinuousMove(QVector<Frame> positionIn, QVector<double> speedPercent, QVector<double> &timeDurationResult, double cnt);
    int lineContinuousMove_vehiclePosition(bool isPositiveDirection, bool isVelocitySpline, QVector<VehiclePosition> positionIn,
                                QVector<double> speedPercent, QVector<double> &timeDurationResult, QVector<double> cnt);
    int lineContinuousMove_vehiclePosition_maccrum( bool isVelocitySpline, QVector<VehiclePosition> positionIn,
                                QVector<double> speedPercent, QVector<double> &timeDurationResult, QVector<double> cnt);
    int splineContinuousMove_vehiclePosition(bool isPositiveDirection, bool isVelocitySpline, QVector<VehiclePosition> positionIn,
                                QVector<double> speedPercent, QVector<double> &timeDurationResult, QVector<double> cnt);
    int splineContinuousMove_vehiclePositionWithControlPoints(bool isPositiveDirection, bool isVelocitySpline,
                                QVector<VehiclePosition> positionIn, QVector<VehiclePosition> middlePos1,
                                                              QVector<VehiclePosition> middlePos2,
                                QVector<double> speedPercent, QVector<double> &timeDurationResult, QVector<double> cnt);
    int splineContinuousMove_vehiclePosition_maccrum( bool isVelocitySpline, QVector<VehiclePosition> positionIn,
                                QVector<double> speedPercent, QVector<double> &timeDurationResult, QVector<double> cnt);
    int circleMove(QVector<Frame>  positionIn, double speedPercent, QVector<double> &timeDurationResult);
    int getVehicleTargetPosition(VehiclePosition &valueOut,int &pointIndexOut);
    int setPathInformation(QVector<VehiclePosition> targetPositionListIn, int pointIndexIn, E_NAV_COMMAND currentPathTypeIn,
                           double currentPathParameter1In=0, double currentPathParameter2In=0);
    int getCurrentPathInformation(VehiclePosition &startPositionOut, QVector<VehiclePosition> &targetPositionOut, int &pointIndexOut, E_NAV_COMMAND &currentPathTypeOut,
                           double &currentPathParameter1Out, double &currentPathParameter2Out);
    int synchronizePositionCommandWithFeedback();
    int getAbsoluteVehiclePositionLeft(double &absoluteDistanceOut, double &absoluteAngleOut);


//    int getLineMoveTimeDuration(Frame positionIn, double speedPercent, double &timeDurationResult);
//    int getLineContinuousMoveTimeDuration(QVector<Frame> positionIn, QVector<double> speedPercent, QVector<double> &timeDurationResult);
//    int getCircleMoveTimeDuration(QVector<Frame>  positionIn, double speedPercent, QVector<double> &timeDurationResult);


    int trapMove(PointPro positionIn,double speedPercent);//PTP

    int trapMove(PointPro positionIn,double speedPercent,double acceleration);//PTP

    int trapMoveByTime(PointPro positionIn,double timeDuration);
    /**
     * @brief rectangularMove  恒定速度运动，速度不超过限制
     * @param positonIn
     * @param speedPercent
     * @return
     */
    int rectangularMove(PointPro positonIn,double speedPercent);
    /**
     * @brief splineZeroMove  样条曲线运动, 起始和终止点速度/加速度为０。
     * 保证限定　速度　不超过最大限制。加速度可能超过。kdl不支持全段限制速度、加速度。
     * @param positionIn 终止点
     * @param speedPercent　最大速度百分比
     * @return
     */
    int splineZeroMove(PointPro positionIn,double speedPercent);
    /**
     * @brief splineZeroMoveByTime  样条曲线运动, 起始和终止点速度/加速度为０。
     * 速度　加速度可能超过最大限制。。kdl不支持全段限制速度、加速度。
     * @param positionIn
     * @param timeDuration
     * @return
     */
    int splineZeroMoveByTime(PointPro positionIn,double timeDuration);
    /**
     * @brief splineContinueMove 样条曲线运动, 终止点速度/加速度为参数值。
     * 保证限定　速度　不超过最大限制。加速度可能超过。kdl不支持全段限制速度、加速度。
     * @param positionIn　终止点
     * @param velIn　终止点速度
     * @param accIn　终止点加速度
     * @return
     */
    int splineContinueMove(PointPro positionIn,double velIn, double accIn);//平滑拼接用

    int splineContinueMoveMultiPoint(QVector<VelCoordinatePoint> velPoint);//多点通过
    /**
     * @brief splineContinueMoveByTime 样条曲线运动, 终止点速度/加速度为参数值。
     * 速度　加速度可能超过最大限制。。kdl不支持全段限制速度、加速度。
     * @param positionIn
     * @param velIn
     * @param accIn
     * @param timeDuration
     * @return
     */
    int splineContinueMoveByTime(PointPro positionIn,double velIn, double accIn, double timeDuration);//平滑拼接用

    /**
     * @brief getTimeDurationTrapMove 获取轨迹运动所需要的时间，多轴同步用。
     * @param positionIn
     * @param speedPercent
     * @param timeDuration
     * @return
     */
    int getTimeDurationTrapMove(PointPro positionIn, double speedPercent, double &timeDuration);//PTP同步用
    int getTimeDurationSplineZeroMove(PointPro positionIn,double speedPercent, double  &timeDuration);//PTP同步用
    int getTimeDurationSplineContinueMove(PointPro positionIn,double velIn, double &timeDuration);//平滑同步用


    /**
     * @brief resetTraj 仅清除历史轨迹数据
     * @return
     */
//    int resetTraj();//
    /**
     * @brief resetAll 清除历史轨迹数据、错误状态、从新加载axis命令值
     * @return
     */
    int resetAll();//
    /**
     * @brief calculateFollowOffset 计算跟随误差
     * @return
     */
    int calculateFollowOffset();
    /**
     * @brief waitPositionFollowOffset  等待跟随误差直到　fabs(positionFollowOffset)小于targetFollowError
     * @return
     */
    int waitPositionFollowOffset();//

    bool isInFineMoving();//是否为最后一条正在运行的轨迹
    /**
     * @brief isVelProfileEmpty  轨迹对象是否为空
     * @return ０有轨迹　１无轨迹
     */
    int isVelProfileEmpty();
    /**
     * @brief isPositionCommandCheckOk Position限位检查
     * @param positionIn
     * @return 　-1 not OK       1 OK
     */
    int isPositionCommandCheckOk(PointPro positionIn);
    /**
     * @brief isVelCommandCheckOk 限位检查
     * @param velIn
     * @return
     */
    int isVelCommandCheckOk(double velIn);
    int isVelBiasCheckOk(Twist velCommandIn, Twist velFeedbackIn);
    /**
     * @brief isAccCommandCheckOk 限位检查
     * @param accIn
     * @return
     */
    int isAccCommandCheckOk(double accIn);
    /**
     * @brief isTorqueCommandCheckOk 限位检查
     * @param torqueIn
     * @return
     */
    int isTorqueCommandCheckOk(double torqueIn);
      int isTorqueFeedbackCheckOk();
    /**
     * @brief isFollowOffsetCheckOk 限位检查
     * @param followOffsetIn
     * @return
     */
    int isFollowOffsetCheckOk(PointPro followOffsetIn);
    /**
     * @brief isTargetOffsetCheckOk 限位检查。轨迹末尾有需要才去调用。
     * @param followOffsetIn
     * @return
     */
    int isTargetOffsetCheckOk(PointPro followOffsetIn);
    int isTargetOffsetCheckOk();
    /**
     * @brief isLimitCheckOk 　位置、速度、加速度、力矩、跟随误差　同时检测，且＆＆
     * @return 　-1 not OK       1 OK
     */
    int isLimitCheckOk();
    /**
     * @brief setCheckMode 　速度检查模式设定
     * @param modeIn　E_CHECK_MODE_T1　　E_CHECK_MODE_T2
     * @return
     */
    int setCheckMode(int modeIn);

    double getAccumulatedTime(int trajIndexIn=0);
    double accumulatedTimeMoveOneStep(int trajIndexIn=0);
    int setAccumulatedTime(double timeIn,int trajIndexIn=0);

    Frame getPositionFrameCommand();
    VehiclePosition getVehiclePositionCommand();
    VehiclePosition getVehiclePositionFeedback();
    VehiclePosition transferFrameToVehiclePosition(Frame frameIn);
    int setPositionFrameFeedback(Frame valueIn);
    int setVehiclePositionFeedback(VehiclePosition valueIn, QVector<double> steeringPositionIn);

    int setPositionFrameCommand(Frame valueIn);
    int setPositionFrameCommand_planed(Frame valueIn);

private:
    /**
     * @brief angleRotationJudgement 考虑哪种角度旋转方法最近
     * @param targetAngle .比较的角度必须要-M_PI  M_PI之间。　否则结果可能很大
     * @param initialAngle
     * @return
     */
    double angleRotationJudgement_improved(double targetAngle,double initialAngle);
    //agv用
    int multiLineSmoothLinePlan(VehiclePosition startPos, QVector<VehiclePosition> passPosList,
                                double speedRatioIn, double cnt, QVector<Trajectory*> &trajListOut);
    int bezierMove_noRotate(E_LINE_VELOCITY_STYLE velStyle,VehiclePosition startPos, VehiclePosition endPos, double speedRatioIn,
                                   QVector<Trajectory *> &trajListOut);
    int bezierMove_noRotateWithControlPoint(E_LINE_VELOCITY_STYLE velStyle,VehiclePosition startPos, VehiclePosition endPos,
                                            VehiclePosition pos1, VehiclePosition pos2,double speedRatioIn,
                                   QVector<Trajectory *> &trajListOut);
    //not used 为了速度平滑，使用了３个线段拼接，后面替换掉，直接适用改进样条曲线速度规划。
    int singleSmoothLinePlan(VehiclePosition startPos,VehiclePosition endPos, double speedRatioIn,QVector<Trajectory*> &trajListOut);
    //替代singleSmoothLinePlan
    int improvedSplineLinePlan(E_LINE_VELOCITY_STYLE velStyle,VehiclePosition startPos, VehiclePosition endPos,
                                                           double speedRatioIn, Trajectory **trajOut);
    /**
     * @brief splineLinePlan //纯粹旋转不能用spline，时间计算不对。not used
     * @param isVelocitySpline
     * @param startPos
     * @param endPos
     * @param speedRatioIn
     * @param trajOut
     * @return
     */
    int splineLinePlan(bool isVelocitySpline, double timeDurationEstimateRatio, VehiclePosition startPos, VehiclePosition endPos,
                       double speedRatioIn, Trajectory **trajOut);
    //not used
    int splineLinePlan_timeDuration(double timeDurationEstimateRatio, VehiclePosition startPos,
                                    VehiclePosition endPos, Trajectory **trajOut);
    int splineRotatePlan(bool isVelocitySpline, VehiclePosition startPos, VehiclePosition endPos,
                       double speedRatioIn, Trajectory **trajOut);

    /**
     * @brief steeringRotatePlan
     * @param startAngle   degree 开始角度
     * @param isPrepareRotation　是否为旋转准备
     * @param trajOut
     * @return
     */
    int steeringRotatePlan(double startAngle, bool isPrepareRotation, Trajectory **trajOut,
                           double &targetAngleOut);
    /**
     * @brief waitTrajectoryPlan 等待轨迹，不运动
     * @param waitTimeIn 等待时间s
     * @param trajOut
     * @return
     */
    int waitTrajectoryPlan(VehiclePosition stationaryPose,double waitTimeIn, Trajectory **trajOut);

    Frame dpointToFrame(const DPoint &pointIn);
    Frame vehiclePositionToFrame(const VehiclePosition &pointIn);
//    DPoint frameToDPoint(const Frame &frameIn);// todo
    double transformToPi(double angleIn);
    int setPositionFrameFeedback(VehiclePosition valueIn);
    int isTrajComposeEmpty();
    int deleteTrajComposite();
//    int deleteTrajComposite_front();
    KDL::Trajectory_Composite* getTrajComposite(int trajIndex=0);

    int addNewTrajComposite(TrajeAttribute attrIn);
    int setTrajCompositeEndCutoffTime(QVector<double> timeIn);
    int addMsg(int messageLevel,
                std::string componentName,
                std::string messageType,
                int messageCode,
                int robotIdIn,
                double parameter1 = 0,
                double parameter2 = 0,
                double parameter3 = 0,
                int parameter4 = 0);


public:
    volatile int limitCheckMode;//E_CHECK_MODE_T1　　E_CHECK_MODE_T2
    QQueue<TrajeAttribute> trajAttributeQueue;
//     KDL::Trajectory_Composite* volatile trajComposite;//deltaTime单位影响
//     KDL::Trajectory_Composite* volatile trajCompositeArry[VEL_PROFILE_ARRY_MAX];//速度队列
//     volatile int currentTrajCompositeArryIndex;//不用队列时＝－１，用队列时０～VEL_PROFILE_ARRY_MAX

    volatile double timeAxisScale;//时间轴比例尺，内部求点时，需要修正v=vOLD/timeAxisScale, a=aOLD/timeAxisScale^2 .
    double timeAxisLimitRatio;
    volatile double axisAccelerationRatio;//加速度比例
    volatile double deltaTime;//单步长
//    double accumulatedTime;//累计时长todo
//    double accumulatedTime_secondTraj;//累计时长
    volatile int axisAbstractType;//轴抽象类型

    volatile int currentRunningLine;//当前运行行
    volatile int iscurrentAxisCommandFInished;//当前运行行是否结束
    volatile double currentCommandRemainingTime;//当前运行行剩余时间
    volatile double currentCommandRemainingTime_secondTraj;//当前运行行剩余时间
    volatile double totalRemainingTime;//总剩余时间,not used
     QQueue<int> linesNumberQueue;//行号队列。
     QQueue<double> linesDurationQueue;//行运行时间队列
     QQueue<SegmentAtrribute> segmentAtrributeQueue;//小段属性队列

public:
    QMutex pathInfoMutex;
    VehiclePosition targetPosition;//路径目标点　m  rad
    QVector<VehiclePosition> targetPositionList;//路径目标点　m  rad
    int targetPointIndex;
    VehiclePosition startPosition;//路径开始点
    E_NAV_COMMAND currentPathType;//路径类型
    double currentPathParameter1;//路径参数１
    double currentPathParameter2;//路径参数２
    PointPro positionCommand;//位置命令 mm degree.没有每一步更新。
    Frame positionFrameCommand;
    Frame positionFrameCommand_planed;//规划起点
    Frame positionFrameCommand_cnt;
    Frame positionFrameCommand_mixed;
    VehiclePosition vehiclePositionCommand;
    VehiclePosition vehiclePositionCommand_mixed;
    PointPro positionCommandOld;//位置命令 mm degree
    Frame positionFrameCommandOld;
    VehiclePosition vehiclePositionCommandOld;
    Twist velCommand;//速度命令 mm/s degree/s
    Twist velCommand_mixed;//速度命令 mm/s degree/s
    Twist accCommand;//速度命令 mm/s^2 degree/s^2
    Twist accCommand_mixed;//速度命令 mm/s^2 degree/s^2
    Twist torqueCommand;//力矩命令　 N Ｎ.mm
    PointPro positionFeedback;//反馈位置 mm degree,,, not use
    Frame positionFrameFeedback;
    VehiclePosition vehiclePositionFeedback;
    double steeringPositionFeedback[10];
    Twist velFeedback;//反馈速度 mm/s degree/s, wrong ,m/s
    Twist torqueFeedback;//反馈力  N Ｎ.mm
    PointPro positionFollowOffset;//位置跟随误差　mm degree
    PointPro positionFrameFollowOffset;
    Twist velFollowOffset;//速度跟随误差　mm/s degree/s
    Twist torqueFollowOffset;//力矩跟随误差　N Ｎ.mm

     volatile int basicStatus;//基本状态
     volatile int axisIdByRobot;//机器人关节顺序命名的轴号，axid从０开始
     volatile int isPositionCommandExceed;
     volatile int isVelCommandExceed;
     volatile int isAccCommandExceed;
     volatile int isTorqueCommandExceed;
     volatile int isProcessFollowErrorExceed;


    //-------------------------------------------------------------轴基本parameter-------------------------------
    /**
     * @brief jointName 关节名称
     */
    std::string jointName;//在公共目录下，使用std::string
    /**
     * @brief jointType 关节类型
     * ０    旋转关节
     * １    移动关节
     */
    int jointType;
    /**
     * @brief dhType
     * 0    standard dh
     * 1    modified dh
     */
    int dhType;//华磊添加　dh参数类型
    /**
     * @brief dhParameter
     * \sita    Z轴之间的夹角
     * \d       Z轴方向的偏移量
     * \a       X轴方向的偏移量
     * \alpha   X轴之间的夹角
     */
    double dhParameter[4];//华磊修改　把原来的QVector<float>dhParameter改为double dhParameter[4]
    //华磊修改　把原来的QVector<float>positionVary改为两个变量maxPosition和minPosition
    /**
     * @brief maxPosition 电机转角/位移最大值，单位角度/mm
     */
    PointPro maxPosition;
    /**
     * @brief minPosition 电机转角/位移最小值，单位角度/mm
     */
    PointPro minPosition;
    /**
     * @brief positionOffset 电机位置偏移
     */
    PointPro positionOffset;
    double lastCommand_velocity_line;//上次轨迹的终点线速度。
    /**
     * @brief maxVelocity 最大转速
     */
    float maxVelocity_line;
    float maxVelocity_rotate;//rad/s, 样条曲线检查旋转使用
    double rotate_eqradius;//旋转速度等效半径
    /**
     * @brief maxTorque 最大转矩
     */
    float maxTorque;

    float jogTorqueLimit;
    /**
     * @brief maxAccelation 最大加速度
     */
    float maxAccelation_line;//mm/s
    double accelerationRatio;
    float maxAccelation_rotate;//rad/s^2 not used
    /**
     * @brief mass 关节质量
     */
    float mass;
    /**
     * @brief massCenter 关节质心
     * x y z
     */
    QVector<float>massCenter;
    /**
     * @brief inertiaMatrix 惯性张量矩阵
     * Ixx Iyy Izz Ixy Ixz Iyz
     */
    QVector<float>inertiaMatrix;
    /**
     * @brief viscousFriction 粘性摩擦常量
     */
    float viscousFriction;
    /**
     * @brief coulombFriction 库伦摩擦常量
     */
    float coulombFriction;
    /**
     * @brief mobile 能否转动
     * true 能转动
     * false 不能转动
     */
    bool mobile;
    /**
     * @brief gearRatio 电机齿轮比
     */
    float gearRatio;
    /**
     * @brief screwPitch 螺距，单位毫米
     */
    float screwPitch;
    /**
     * @brief encoderResolution 电机编码器位数
     */
    int encoderResolution;
    /**
     * @brief pulsePerRound 电机一圈的脉冲数，初始化时计算
     */
    int pulsePerRound;//华磊添加
    /**
     * @brief coupleFlag 耦合标记,其他关节是否对其产生作用
     */
    bool coupleFlag;
    /**
     * @brief coupleJoint 耦合作用的关节索引(该索引为原始影响关节)，从0开始
     */
    int coupleJoint;
    /**
     * @brief coupleReducer 对耦合作用有影响的减速比（关节　影响　关节。关节　影响　直线。　该参数２种情况都适用。正负关联补偿方向。关节　影响　直线　的情况，参数为-1 或１。）
     */
    float coupleReducer;
    /**
     * @brief 外部轴标识,是否外部轴, 0:内部轴 1:外部轴
     */
    int externAxisFlag;
    /**
     * @brief 指令精确到达的目标点允许误差
     */
    float targetFollowError;//20170110LFD改为int，单位是脉冲
    /**
     * @brief 跟隨過程最大允许误差
     */
    float processFollowError;//2017011LFD添加，单位是脉冲

    double t1RatioLimit;

    double allowVelFeedbackBiasRatio;

    double noRotateMoveAngle;//rad, 不旋转运动角度


   //-------------------------------------------------------------轴基本parameter-------------------------------

private:
    MotionMessage* motionMessage;


    int robotId;
    double steeringMaxSpeed;//degree/s
    double steeringMaxAcceration;//degree/s^2
    bool isSteeringValid;//转向过渡是否生效
    E_VELOCITY_TYPE velocityStype;
    int tmp_exceedCount;
    double tmp_velFeedback_old;
    bool isInPositionMixsing;
    double positionMixRatio;
};

#endif // COORDINATETRAJECTORY_H
